/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*******************************************************************************/

#include <nrk_defs.h>
#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <hal.h>
#include <nrk_error.h>
#include <nrk_timer.h>
#include <nrk_stack_check.h>
#include <nrk_stats.h>

//This execution time should fail in a rate monotonic system
#define TASK1_EXEC 2
#define TASK2_EXEC 4
#define TASK3_EXEC 4

#define TASK1_PERIOD 7 
#define TASK2_PERIOD 13 
#define TASK3_PERIOD 10

#define TASK1_PRIO 3
#define TASK2_PRIO 1
#define TASK3_PRIO 2

#define SPIN_WAIT_TIME 908 // every second we wait, we only do 908 ms, any higher and we get unwanted task reserve errors...

NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);

NRK_STK Stack2[NRK_APP_STACKSIZE];
nrk_task_type TaskTwo;
void Task2 (void);

NRK_STK Stack3[NRK_APP_STACKSIZE];
nrk_task_type TaskThree;
void Task3 (void);

inline void spin_wait_s(uint16_t wait_sec) {
  int16_t s_cnt, ms_cnt;
    for (s_cnt = 0; s_cnt < wait_sec; s_cnt++) {
        for (ms_cnt = 0; ms_cnt < SPIN_WAIT_TIME; ms_cnt++) {
            //spinwait 1ms
            nrk_spin_wait_us(1000);
        }
    }
}

inline void compare_and_print(uint16_t arrival, uint16_t finish, uint16_t period, uint16_t cnt) {
  uint8_t task_ID = nrk_get_pid();
  printf("T%u cnt:%u, arr:%u end:%u\r\n", task_ID, cnt, arrival, finish);
  if( finish - arrival > period ) {
      printf("Task %u has missed it's deadline\r\n", task_ID);
  }
}

void nrk_create_taskset();

int
main ()
{
  nrk_setup_ports();
  nrk_setup_uart(UART_BAUDRATE_115K2);

  nrk_init();

  nrk_led_clr(ORANGE_LED);
  nrk_led_clr(BLUE_LED);
  nrk_led_clr(GREEN_LED);
  nrk_led_clr(RED_LED);
 
  nrk_time_set(0,0);
  nrk_create_taskset ();

  nrk_kprintf( PSTR("Nano-RK Version ") );
  printf( "%d\r\n",NRK_VERSION );


  nrk_start();
  
  return 0;
}


void Task1()
{
nrk_time_t t;

uint16_t cnt = 0;
uint16_t i;
uint8_t task_ID = nrk_get_pid();

while(1) {
  nrk_wait_until_next_period();
  spin_wait_s(TASK1_EXEC);
  nrk_time_get(&t);
  compare_and_print(task_arrival_time[task_ID], t.secs, TASK1_PERIOD,cnt);
  cnt++;
}
}

void Task2()
{
nrk_time_t t;

uint16_t cnt = 0;
uint16_t i;
uint8_t task_ID = nrk_get_pid();

while(1) {
  nrk_wait_until_next_period();
  spin_wait_s(TASK2_EXEC);
  nrk_time_get(&t);
  compare_and_print(task_arrival_time[task_ID], t.secs, TASK2_PERIOD, cnt);
  cnt++;
}
}

void Task3()
{
nrk_time_t t;

uint16_t cnt = 0;
uint16_t i;
uint8_t task_ID = nrk_get_pid();

while(1) {
  nrk_wait_until_next_period();
  spin_wait_s(TASK3_EXEC);
  nrk_time_get(&t);
  compare_and_print(task_arrival_time[task_ID], t.secs, TASK3_PERIOD, cnt);
  cnt++;
}
}


void
nrk_create_taskset()
{
  nrk_task_set_entry_function( &TaskOne, Task1);
  nrk_task_set_stk( &TaskOne, Stack1, NRK_APP_STACKSIZE);
  TaskOne.prio = TASK1_PRIO;
  TaskOne.FirstActivation = TRUE;
  TaskOne.Type = BASIC_TASK;
  TaskOne.SchType = PREEMPTIVE;
  TaskOne.period.secs = TASK1_PERIOD;
  TaskOne.period.nano_secs = 0;
  TaskOne.cpu_reserve.secs = TASK1_EXEC;
  TaskOne.cpu_reserve.nano_secs = 0;
  TaskOne.offset.secs = 0;
  TaskOne.offset.nano_secs= 0;
  nrk_activate_task (&TaskOne);

  nrk_task_set_entry_function( &TaskTwo, Task2);
  nrk_task_set_stk( &TaskTwo, Stack2, NRK_APP_STACKSIZE);
  TaskTwo.prio = TASK2_PRIO;
  TaskTwo.FirstActivation = TRUE;
  TaskTwo.Type = BASIC_TASK;
  TaskTwo.SchType = PREEMPTIVE;
  TaskTwo.period.secs = TASK2_PERIOD;
  TaskTwo.period.nano_secs = 0;
  TaskTwo.cpu_reserve.secs = TASK2_EXEC;
  TaskTwo.cpu_reserve.nano_secs = 0; 
  TaskTwo.offset.secs = 0;
  TaskTwo.offset.nano_secs= 0;
  nrk_activate_task (&TaskTwo);

  nrk_task_set_entry_function( &TaskThree, Task3);
  nrk_task_set_stk( &TaskThree, Stack3, NRK_APP_STACKSIZE);
  TaskThree.prio = TASK3_PRIO;
  TaskThree.FirstActivation = TRUE;
  TaskThree.Type = BASIC_TASK;
  TaskThree.SchType = PREEMPTIVE;
  TaskThree.period.secs = TASK3_PERIOD;
  TaskThree.period.nano_secs = 0;
  TaskThree.cpu_reserve.secs = TASK3_EXEC;
  TaskThree.cpu_reserve.nano_secs = 0;
  TaskThree.offset.secs = 0;
  TaskThree.offset.nano_secs= 0;
  nrk_activate_task (&TaskThree);
}

uint8_t kill_stack(uint8_t val)
{
char bad_memory[10];
uint8_t i;
for(i=0; i<10; i++ ) bad_memory[i]=i;
for(i=0; i<10; i++ ) printf( "%d ", bad_memory[i]);
   printf( "Die Stack %d\r\n",val );
if(val>1) kill_stack(val-1);
return 0;
}


